/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#include <sxMSM_IG_InverseVelocityResponseHandler.h>

const static float DEFAULT_ENERGY_LOSS_COEF = 0.05f;
const static float DEFAULT_FRICTION_COEF = 0.01f;

Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::MSM_IG_InverseVelocityResponseHandler() {
	this->energyLossCoef = DEFAULT_ENERGY_LOSS_COEF;
	this->frictionCoef = DEFAULT_FRICTION_COEF;
}

Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::~MSM_IG_InverseVelocityResponseHandler() {}

bool Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::construct(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects) {
	std::cout << "[MSM_IG_InverseVelocityResponseHandler] construct" << std::endl;
	return true;
}

bool Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::update(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects, Physics::Collision::Aggregates::InfiniteGroundCollisionEventAggregate<Objects::MassSpringMesh>* const collisionData) {
	return true;
}

bool Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::performResponse(Util::ArrayList<std::shared_ptr<MassSpringMesh> >& objects, Physics::Collision::Aggregates::InfiniteGroundCollisionEventAggregate<Objects::MassSpringMesh>* const collisionData, bool b) {
	//--------------------------------------------------------------------------
	// Iterate through each event. Since the infinite-ground collision event
	// is only detected between a vertex in an object and the ground (face)
	// the vertex-face event pool is used. Each event will be handled by
	// inversing the velocity and adjusting the position of the vertex.
	//--------------------------------------------------------------------------
	Eigen::Vector3d position, velocity;
	for ( unsigned int i = 0; i < collisionData->eventpool_VertexFace.size(); i++ ) {
		Physics::Collision::Event::Events::HeightCollisionEvent<Objects::MassSpringMesh>& heightEvent = collisionData->eventpool_VertexFace[i];
		std::shared_ptr<Objects::MassSpringMesh> curMesh = heightEvent.object;

		if ( curMesh == nullptr ) {
			std::cerr << "[MSM_IG_InverseVelocityResponseHandler:performResponse] Error: Nullptr mesh encountered." << std::endl;
			std::cerr << "  Aborting collision response." << std::endl;
			return false;
		}

		unsigned int vertexIndex = heightEvent.vertexIndex;
		float height = heightEvent.height;
		position = curMesh->getNodePosition(vertexIndex);
		velocity = curMesh->getNodeVelocity(vertexIndex);

		//----------------------------------------------------------------------
		// Provide collision response:
		// Adjust Vertex Position
		// Inverse Velocity
		//----------------------------------------------------------------------
		position.y() = height;

		velocity.y() *= this->energyLossCoef;
		velocity.x() *= this->frictionCoef;
		velocity.z() *= this->frictionCoef;

		velocity *= -1.0;

		curMesh->setNodePosition(vertexIndex, position);
		curMesh->setNodeVelocity(vertexIndex, velocity);
	}
	
	return true;
}

void Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::setEnergyLossCoef(float coefficient) {
	this->energyLossCoef = coefficient;
}

void Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::setFrictionCoef(float coefficient) {
	this->frictionCoef = coefficient;
}

float Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::getEnergyLossCoef() const {
	return this->energyLossCoef;
}

float Sx::Simulation::Collision::Handlers::Response::MSM_IG_InverseVelocityResponseHandler::getFrictionCoef() const {
	return this->frictionCoef;
}
